

#include <std.h>
#include <stdio.h>
#include <string.h>
#include <log.h>
#include <tsk.h>
#include <ecm.h>
#include <gio.h>
#include "ti/pspiom/i2c/I2c.h"
#include "..\libMath\matrix.h"

#include "ITG3205.h"
#include "err.h"
#include "common.h"
#include "stdio.h"
#include "math.h"
#include "common_i2c.h"
#include "navconbioscfg.h"

//The unit before and after compensation is 'deg/s'
double gyro_cal_coe[12] = {	1.00199,		0.00344600,		-0.0244674,
							-0.00385557,	0.999717,		-0.00751126,
							-0.0166068,		0.0115883,		1.00941,
							0.764737,		2.65165,		-0.426447};

double gyro_caltp_coe[12] = {	7.52270e-07,	2.77706e-05,	2.56746e-06,
								-3.40790e-04,	-0.000591567,	-6.04695e-06,
								-0.0198256,		-0.104000,		0.0317978,
								0.220417,		0.0956675,		-0.372408};

/* ========================================================================== */
/*                           FUNCTION DEFINITIONS                             */
/* ========================================================================== */

/* \fn
 *
 * \brief   Function to ITG3205 three-axis Gyro driver.
 *
 */


/* read register values start from address addr, length provided by user */
int ITG3205_READ_REG(GIO_Handle inhd,GIO_Handle outhd, Uint8 REG, Uint8 *buf, size_t len)
{
	int error = 0;
	//request to read register @ address *addr
	error = i2cWrite(outhd,ITG3205_ADDR,( unsigned char *)&REG,1,I2c_DEFAULT_WRITE);
	if(error!=IOM_COMPLETED){
		return error;
	}
	//perform read, write to user-defined buffer
    error = i2cRead(inhd,ITG3205_ADDR,( unsigned char *)buf,len,I2c_DEFAULT_READ);
	if(error!=IOM_COMPLETED){
		return error;
	}
	return error;
}

/* write one byte to register and return 0 if success. */
int ITG3205_WRITE_REG(GIO_Handle inhd,GIO_Handle outhd, Uint8 REG,Uint8 value, op_mode op)	//write a byte to reg, and return if operation is success. 0--success, 1--fail.
{

	int error = 0;
	Uint8 temp[2];

	temp[0] = REG;
	temp[1] = value;
	error = i2cWrite(outhd,ITG3205_ADDR,( unsigned char *)temp,2,I2c_DEFAULT_WRITE);
	if(error!=IOM_COMPLETED){
		return error;
	}

	if(op == VALIDATE){
		error = ITG3205_READ_REG(inhd,outhd,REG,temp,1);
		if(error){
			//I2C bus error
		}else if(temp[0] != value){
			error = IO_VALIDATED_ERR;
		}
	}

 	return error;
}


/* Return INVALID_ID if error */
int ITG3205_CONNECT(GIO_Handle inhd,GIO_Handle outhd)		//Connect to ITG3205, read ID and identify
{
	Uint8 id;
	int error = 0;
    error = ITG3205_READ_REG(inhd,outhd, ITG3205_RA_WHO_AM_I, &id, 1);
	if(error){
		//I2C bus error
		return error;
	}
	if (id != 0x68) {
		error = INVALID_ID;
    }else{
    	error = 0;
    }
	return error;
}


/* ITG3205_SET_LPF
 * This function is required for proper operation,
 * it sets the gyro to operate at full scale and
 * configure the LPF frequency
 * */
int ITG3205_SET_LPF(GIO_Handle inhd,GIO_Handle outhd, Uint8 LPF_val){
	int error = 0;
	Uint8 temp = 0;
	temp = FULLSCALE_SEL + LPF_val;
	error = ITG3205_WRITE_REG(inhd,outhd, ITG3205_RA_DLPF_FS, temp, VALIDATE);

	return error;
}
/* ITG3205_SET_SRD
 * Set Sample Rate Divider:
 * F_sample = F_internal/(divider+1),
 * where F_internal = 1kHz or 8kHz
 * depends on the LPF setting.
 * */
int ITG3205_SET_SRD(GIO_Handle inhd,GIO_Handle outhd, Uint8 SRD_val){
	int error = 0;
	Uint8 temp = 0;
	temp = SRD_val;
	error = ITG3205_WRITE_REG(inhd,outhd, ITG3205_RA_SMPLRT_DIV, temp, VALIDATE);

	return error;

}

//Set INT configuration
/*****************************************
Bit7	Bit6	Bit5	Bit4	Bit3	Bit2	Bit1	Bit0
ACTL	OPEN	LAIE	INA2	0		ITRE	0		RARE
ACTL --	Logic level for INT output pin 1=active low, 0=active high
OPEN --	Drive type for INT output pin 1=open drain, 0=push-pull
LATCH_INT_EN --	Latch mode 1=latch until interrupt is cleared, 0=50us pulse
INT_ANYRD_2CLEAR --	Latch clear method 1=any register read, 0=status register read only
ITG_RDY_EN -- Enable interrupt when device is ready (PLL ready after changing clock source)
RAW_RDY_EN -- Enable interrupt when data is available
0 -- Load zeros into Bits 1 and 3 of the Interrupt Configuration register.
*******************************************/
int ITG3205_INT_CFG(GIO_Handle inhd,GIO_Handle outhd, Uint8 int_cfg)
{
	return ITG3205_WRITE_REG( inhd, outhd, ITG3205_RA_INT_CFG, int_cfg, VALIDATE);
}

//Get ITG3205 INTSTATUS,bit0--RawdataRDY,bit2--ITG RDY
int ITG3205_READ_INT_STATUS(GIO_Handle inhd,GIO_Handle outhd, Uint8 * buf)
{
	return ITG3205_READ_REG(inhd,outhd, ITG3205_RA_INT_STATUS, buf, 1);
}

//1--INT output active Low, 0--INT output active High
int ITG3205_SET_INT_LogicLV(GIO_Handle inhd,GIO_Handle outhd, Bool type)
{
	Uint8 temp = 0;
	int status = 0;

	status = ITG3205_READ_REG(inhd,outhd, ITG3205_RA_INT_CFG, &temp, 1);
	if(status){
		return status;
	}
	temp = temp & ~ ACTL | type << 7 ;

	status = ITG3205_INT_CFG( inhd, outhd, temp);

	return status;
}

//1--any reg read, 0--status reg read only
int ITG3205_SET_INT_DriveType(GIO_Handle inhd,GIO_Handle outhd,Bool type)
{
	Uint8 temp = 0;
	int status = 0;
	status = ITG3205_READ_REG(inhd,outhd, ITG3205_RA_INT_CFG, &temp, 1);
	if(status){
		return status;
	}
	temp = temp & ~ OPEN_DRAIN | type << 6;
	status = ITG3205_INT_CFG( inhd, outhd, temp);

	return status;
}

int ITG3205_SET_INT_LatchMode(GIO_Handle inhd,GIO_Handle outhd, Bool type){
	Uint8 temp = 0;
	int status = 0;
	status = ITG3205_READ_REG(inhd,outhd, ITG3205_RA_INT_CFG, &temp, 1);
	if(status){
		return status;
	}
	temp = temp & ~ LATCH_INT_EN | type << 5;
	status = ITG3205_INT_CFG( inhd, outhd, temp);

	return status;
}

int ITG3205_SET_INT_LatchClrMode(GIO_Handle inhd,GIO_Handle outhd, Bool type){
	Uint8 temp = 0;
	int status = 0;
	status = ITG3205_READ_REG(inhd,outhd, ITG3205_RA_INT_CFG, &temp, 1);
	if(status){
		return status;
	}
	temp = temp & ~ INT_ANYRD_2CLEAR | type << 4;
	status = ITG3205_INT_CFG( inhd, outhd, temp);

	return status;
}

//1--enable interrupt when device is ready, 0--disable
int ITG3205_SET_INT_ITGRDY(GIO_Handle inhd,GIO_Handle outhd, Bool int_itgrdy)
{
	Uint8 temp = 0;
	int status = 0;

	status = ITG3205_READ_REG(inhd,outhd, ITG3205_RA_INT_CFG, &temp, 1);
	if(status){
		return status;
	}
	temp = temp & ~ ITG_RDY_EN | int_itgrdy << 2;
	status = ITG3205_INT_CFG( inhd, outhd, temp);

	return status;
}

//1--enable interrupt when data is ready, 0--disable
int ITG3205_SET_INT_RAWRDY(GIO_Handle inhd,GIO_Handle outhd, Bool int_rawrdy)
{
	Uint8 temp = 0;
	int status = 0;
	status = ITG3205_READ_REG(inhd,outhd, ITG3205_RA_INT_CFG, &temp, 1);
	if(status){
		return status;
	}
	temp = temp & ~ RAW_RDY_EN | int_rawrdy;
	status = ITG3205_INT_CFG(inhd, outhd, temp);
	return status;
}

//Reset device and internal register to the power-up-default settings
int ITG3205_RESET(GIO_Handle inhd,GIO_Handle outhd)
{
	Uint8 temp = 0;
	int status = 0;

  	status = ITG3205_READ_REG(inhd,outhd, ITG3205_RA_PWR_MGM, &temp, 1);
  	if(status){
		return status;
	}
  	temp = temp | 0x80;
  	status = ITG3205_WRITE_REG( inhd, outhd, ITG3205_RA_PWR_MGM, temp, NORMAL);
  	return status;
}

//Enable low power sleep mode
int ITG3205_SLEEP(GIO_Handle inhd,GIO_Handle outhd)
{
	Uint8 temp = 0;
	int status = 0;

  	status = ITG3205_READ_REG(inhd,outhd, ITG3205_RA_PWR_MGM, &temp, 1);
  	if(status){
		return status;
	}

  	temp = temp | 0x40;

  	status = ITG3205_WRITE_REG( inhd, outhd, ITG3205_RA_PWR_MGM, temp, VALIDATE);
  	return status;
}

//ITG3205 back into normal mode
int ITG3205_WAKEUP(GIO_Handle inhd,GIO_Handle outhd)
{
	Uint8 temp = 0;
	int status = 0;

  	status = ITG3205_READ_REG(inhd,outhd, ITG3205_RA_PWR_MGM, &temp, 1);
  	if(status){
		return status;
	}
  	temp = temp | 0xBF;
  	status = ITG3205_WRITE_REG( inhd, outhd, ITG3205_RA_PWR_MGM, temp, VALIDATE);
  	return status;
}


//ITG3205 axes power mode setting, 1--standby mode, 0--normal mode
int ITG3205_SET_AXES_PWR_MODE(GIO_Handle inhd,GIO_Handle outhd, Uint8 x_pwm,Uint8 y_pwm,Uint8 z_pwm)
{
	Uint8 temp = 0;
	int status = 0;
  	status = ITG3205_READ_REG(inhd,outhd, ITG3205_RA_PWR_MGM, &temp, 1);
  	if(status){
		return status;
	}
  	if(x_pwm == 0x01){
  		temp = temp | 0x20;
  	}else{
  		temp = temp & 0xDF;
  	}
  	if(y_pwm == 0x01){
  		temp = temp | 0x10;
  	}else{
  		temp = temp & 0xEF;
  	}
  	if(z_pwm == 0x01){
  		temp = temp | 0x08;
  	}else{
  		temp = temp & 0xF7;
  	}
  	status = ITG3205_WRITE_REG( inhd, outhd, ITG3205_RA_PWR_MGM, temp, VALIDATE);
  	return status;
}


int ITG3205_CLKSEL(GIO_Handle inhd,GIO_Handle outhd, Uint8 clk_source)
{
	Uint8 temp = 0;
	int status = 0;
	if(clk_source < 6){
	  	status = ITG3205_READ_REG(inhd,outhd, ITG3205_RA_PWR_MGM, &temp, 1);
	  	if(status){
			return status;
		}
	  	temp = temp & 0xF8;
	  	temp = temp + clk_source;
	  	status = ITG3205_WRITE_REG( inhd, outhd, ITG3205_RA_PWR_MGM, temp, VALIDATE);
	}else{
		status = INVALID_ARG;
	}
	return status;
}



int ITG3205_GET_RDATA(GIO_Handle inhd,GIO_Handle outhd, short *rdata)
{
	int error = 0;
	Uint8 temp[8];
	Int16 ITG3205_offx = 0;
	Int16 ITG3205_offy = 0;
	Int16 ITG3205_offz = 0;

    error = ITG3205_READ_REG(inhd,outhd, ITG3205_RA_TEMP_OUT_H, temp, 8);
	if(error){
		//I2C bus error
		return error;
	}

    rdata[3] = (((Int16)temp[0]) << 8) | temp[1];
    rdata[0] = (((Int16)temp[2]) << 8) | temp[3];
    rdata[0] = rdata[0] + ITG3205_offx;
    rdata[1] = (((Int16)temp[4]) << 8) | temp[5];
    rdata[1] = rdata[1] + ITG3205_offy;
    rdata[2] = (((Int16)temp[6]) << 8) | temp[7];
    rdata[2] = rdata[2] + ITG3205_offz;

    return error;
}

void ITG3205_SCALE_DATA(short*rdata, double *data)
{
	data[3] = 35 + ((double)(rdata[3] + 13200)) / 280;	//t ..[3]
	data[0] = (double)rdata[0] / 14.375;				//x ..[0]
	data[1] = (double)rdata[1] / 14.375;				//y ..[1]
	data[2] = (double)rdata[2] / 14.375;				//z ..[2]
}

void ITG3205_CAL_DATA(short*rdata, double *data, Uint8 mode)
//rdata-raw data, data-output 3*1, mode
// mode is used for select whether temperature compensation is applied, 0-no temperature compensation, 1-temperature compensated
{
	double temp[4],cal_data[3],delt_tp[4],cal_tp_data[3],temp1[3];

	data[3] = 35 + ((double)(rdata[3] + 13200)) / 280;	//t ..[3]
	temp[0] = (double)rdata[0] / 14.375;				//x ..[0], deg/s
	temp[1] = (double)rdata[1] / 14.375;				//y ..[1]
	temp[2] = (double)rdata[2] / 14.375;				//z ..[2]
    temp[3] = 1;

    //start compensation
    if(mode == 0)
    {
    	MatMul(1,4,3, temp, gyro_cal_coe, cal_data);
    	data[0] = cal_data[0];
    	data[1] = cal_data[1];
    	data[2] = cal_data[2];
    }
    else if(mode == 1)
    {
    	delt_tp[3] = 1;
    	delt_tp[2] = data[3] - 35;
    	delt_tp[1] = delt_tp[2] * delt_tp[2];
    	delt_tp[0] = pow((delt_tp[2]),3);
    	MatMul(1,4,3, temp, gyro_cal_coe, cal_data);
    	MatMul(1,4,3, delt_tp, gyro_caltp_coe, temp1);
    	temp1[0] = -temp1[0];	//multiply -1 with temp1
    	temp1[1] = -temp1[1];
    	temp1[2] = -temp1[2];
    	MatAdd(1, 3, cal_data, temp1, cal_tp_data);
    	data[0] = cal_tp_data[0];
    	data[1] = cal_tp_data[1];
    	data[2] = cal_tp_data[2];
    }
}



